/*
 *  joint.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 22/01/09.
 *
 */
// Joints are not supposed to create points, as bodies do

#include "joint.h"

Joint::Joint()
:	name(""),
	dim(3),
	parentSystem(0),
	vConstraints(0),
	outGraphFilex()
{}

// ***

Joint::Joint(ifstream &in, string cname, MecSystem* pSys)
:	name(cname),
	dim(3),
	parentSystem(pSys),
	vConstraints(0),
	outGraphFilex()
{}

// ***

void Joint::reportContent()
{
	int i;
	for(i=0; i<vConstraints.size(); i++)
	{
		cout << "			Constraint " << i << " with points \n";
		vConstraints[i]->reportContent();
	}
}

// ***

void Joint::setDofs()
{
	for(int i=0; i<vConstraints.size(); i++)	vConstraints[i]->setDofs();
}

// ***

void Joint::setLocalGlob()
{
	for(int i=0; i<vConstraints.size(); i++)	vConstraints[i]->setLocalGlob();
}

// ***

void Joint::setLoad(string lType, ifstream &inn, const vector<double> &vTime, const map<string,vector<string>,less<string> >* pset) 
{}

// ***

void Joint::reportLoads()
{}

// ***

void Joint::initializeGlobalDofs(vector<double> &q, vector<double> &dq, const double& time)
{
	int i;
	for(i=0; i<vConstraints.size(); i++) vConstraints[i]->initializeGlobalDofs(q,dq,time);
}

// ***

void Joint::fillResidual(vector<double>& res_in, vector<double>& q_in, vector<double>& dq_in,const double& time, const int& modeFlag, const double& grav, const int& gravDir, const double& DeltaT)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->fillResidual(res_in, q_in, dq_in, time, modeFlag, grav, gravDir, DeltaT);
}

// ***

void Joint::fillResidual(	vector<double>& residue,
						 const vector<double>& q,
						 const vector<double>& qdot,
						 const vector<double>& qddot,
						 const double& time,
						 const int& modeFlag,
						 const double& grav, const int& gravDir,
 						 const int& integratorType,
						 const double& DeltaT
						 )
{
	int i;
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->fillResidual(residue, q, qdot, time, modeFlag, grav, gravDir, DeltaT);
}

// ***

void Joint::fillResFi(	vector<double>& residue,
					  const vector<double>& q,
					  const vector<double>& qddot,
					  const int& modeFlag
					  )
{}

// ***

void Joint::fillResFq(	vector<double>& residue,
					  const vector<double>& q,
					  const vector<double>& qdot,
					  const int& modeFlag,
  					  const int& integratorType,
					  const double& DeltaT
					  )
{}

// ***

void Joint::fillResFt(	vector<double>& residue,
					  const double& time,
					  const int& modeFlag,
					  const double& grav, const int& gravDir
					  )
{}

// ***

void Joint::fillTangent(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi, 
						vector<int>& globalLocalI, vector<int>& globalLocalB, 
						vector<double>& q_in, vector<double>& dq_in,
						const double& time, const int& modeFlag)
{
	for(int i=0; i<vConstraints.size(); i++)	vConstraints[i]->fillTangent(Kint, Kbb, Kib, Kbi, globalLocalI, globalLocalB, q_in, dq_in, time, modeFlag, 0.0);
}

// ***

void Joint::fillTangent	(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi,
						 vector<int>& globalLocalI, vector<int>& globalLocalB,	
						 const vector<double>& q,
						 const vector<double>& qdot,
						 const vector<double>& qddot,
						 double partial_qdot,
						 double partial_qddot,
						 const double& time,
						 const int& modeFlag,
 						 const int& integratorType,
						 const double& DeltaT
						 )
{
	for(int i=0; i<vConstraints.size(); i++)	vConstraints[i]->fillTangent(Kint, Kbb, Kib, Kbi, globalLocalI, globalLocalB, q, qdot, time, modeFlag, DeltaT);
}

// ***

void Joint::fillMassMatrix(sparseMatrix& Ma, const vector<double>& q, const double& time )
{}

// ***

void Joint::fillMassMatrix(vector<double>& vMa, const vector<double>& q, const double& time )
{}

// ***

void Joint::fillForce(vector<double>& f,
					  const vector<double>& q,
					  const vector<double>& qdot,
					  const double& time,
					  const double& grav, const int& gravDir
					  )
{
	int i;
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->fillForce(f, q, qdot, time, grav, gravDir);
}

// ***

void Joint::setSparsePattern(vector< vector<int> >& tempIa)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)
	{
		vConstraints[i]->setSparsePattern(tempIa);	
	}	
}

// ***

void Joint::setSparsePatternInertia(vector< vector<int> >& tempIa)
{}

// ***

void Joint::changePointName(string& oldName, string& newName)
{
	int i;
	for(i=0; i<vConstraints.size(); i++) vConstraints[i]->changePointName(oldName,newName);
}

// ***

void Joint::writeFileGraph(ofstream& fGraph, string& gFormat, string& gPath, string& aux)
{
	if (!(gFormat.compare("msh")))	writeMSH(fGraph);
	if (!(gFormat.compare("vtu")))
	{
		// Define the localFGraph stream for the body data
		// The format of the filename is systemname-bodyname-proc-numStep.vtu
		string isini;
		outGraphFilex.clear();
		outGraphFilex = gPath + "Graph";
		outGraphFilex+= aux.substr(0,aux.find_last_of("-"));
		outGraphFilex+= "-parts/";
		outGraphFilex+= name;
		outGraphFilex = subsSlash(outGraphFilex);
		outGraphFilex+= "-";
		outGraphFilex+= aux;
		outGraphFilex+= ".vtu";
		
		char *caux = new char [outGraphFilex.size()+1];
		strcpy (caux, outGraphFilex.c_str());
		
		localFGraph.open(caux);
		writeVTU(fGraph);
		localFGraph.close();
		
		// Write the line in the .pvd file
		fGraph << "		<DataSet timestep=\"";
		fGraph << aux.substr(aux.find_last_of("-")+1);
		fGraph << "\" group=\"\" part=\"";
		fGraph << aux.substr(0,aux.find_last_of("-"));
		fGraph << "\" file=\"";
		fGraph << outGraphFilex;
		fGraph << "\"/>\n";		
	}
	if (!(gFormat.compare("oog")))	writeOOGL(fGraph);
}

// ***

void Joint::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{
	int i;
	// WARNING: initializeCompRes changed to initializeConsRes
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->initializeConsRes(st,grav,gravDir,augLagFlag);
}

// ***

void Joint::initializeCompTang(const bool& st)
{
	int i;
	// WARNING: initializeCompTang changed to initializeConsTang
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->initializeConsTang(st);
}

// ***

void Joint::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q, vector<double>& dq)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->calcEner(Ekin, Epot, Econ, Eext, Eamb, Dint, grav, gravDir,q, dq);
}

// ***

void Joint::calcEntropy(double& Entropy, double& EntropyAmb)
{}

// ***

void Joint::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}

// ***

void Joint::calcCon(vector<double>& vcon, vector<double>& q, vector<double>& dq, vector<double>& ddq, double& time)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->calcCon(vcon, q, dq, ddq, time);
}

// ***

void Joint::update(const int& integratorType, const vector<double>& qConv, const vector<double>& dqConv)
{
	int i;		
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->update(integratorType, qConv, dqConv);
}

// ***

void Joint::setInitialConditions(vector<double>& q, vector<double>& dq)
{}

// ***

void Joint::setTemperature(double& theta)
{}

// ***

void Joint::setTRNumbers(int &num)
{}

// ***

void Joint::updateDofs(vector<double>& q, vector<double>& deltaq)
{}

// ***

int Joint::setInertiaNumbers(int num, vector<int>& v)
{
	return num;
}

// ***

void Joint::setQatComponents(const vector<double>& q, const vector<double>& dq)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)		vConstraints[i]->getLocalDofs(q,dq);
}

// ***

void Joint::updateLagMul(void)
{
	int i;	
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->updateLagMul();
}

// ***

void Joint::checkConvLagMul(bool& c)
{
	int i;	
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->checkConvLagMul(c);
}

// ***

void Joint::calcProjMatrix(sparseMatrix& A, double& projPenal)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)	vConstraints[i]->calcProjMatrix(A, projPenal);
}

// ***

void Joint::fillPhiq(vector<vector<double> >& Phiq, int& numd)
{
	int i;
	for(i=0; i<vConstraints.size(); i++)		vConstraints[i]->fillPhiq(Phiq,numd);	
}